/***************************************************************************
 创建者: 华磊
 开始时间: 2019.11.7
 copyright: (C) 华友高科
 修改说明:
 ***************************************************************************
 *  @file xxx.h
 *  功能:
 * 摆动电弧。
 *
 *                                                                         *
 ***************************************************************************/

#ifndef WAVEMOTION_H
#define WAVEMOTION_H
#include <kdl/frames.hpp>
#include "globalDefine.h"
#include "gSoapFoundation/soapStub.h"
using namespace KDL;

class AxisObject;
class MotionMessage;

//坐标轴数据转移到关节轴数据.TODO_waveMove摆弧.TrajectoryServer::waveMoveOneStep()
class WaveMotion
{
public:
    WaveMotion(double deltaTimeIn, MotionMessage *motionMessageIn,double maxLineVelIn, double maxAccIn);

public:

    E_WAVE_STATUS getWaveMotionStatus();
//    int setTimeAxisScale(double ratio);
    /**
     * @brief setWaveType
     * @param typeIn
     * @param waveWidthIn　单位mm
     * @param wavePeriodIn 单位Hz
     * @return
     */
    int setWaveType(E_WAVE_TYPE typeIn, double widthIn, double frequencyIn);
    int setWaveMotionInfo(WeaveMotionCondition infoIn);

    int setWaveAcceleration(double accelerationIn, double maxVelIn, bool isRatio);
    int startWave();
    int stopWave();
    int ceaseWave();
    int moveOneStep(double currentVelRatio, const std::vector<double> &inialPosIn
                    , std::vector<double> &positionOut);
    int resetAll();

private:
//    int setTimeAxisScale(double ratio);
    int sinMove(double widthIn, E_WAVE_TYPE waveTypeIn, const Frame &inialFrameIn,
                Frame &positionFrameOut);
    int initialSpeedRatioAxis();

private:
    double timeAxisScale;//时间轴比例尺，内部求点时，需要修正v=vOLD/timeAxisScale, a=aOLD/timeAxisScale^2 .
    volatile double deltaTime;//单步长
    volatile double accumulatedTime;//累计时长
//    double waveWidth;//震荡幅度ｍ
//    double wavePeriod;//震荡周期s
//    E_WAVE_TYPE waveType;
    double maxLineVel;
    double timeAxisAcceleration;
    double maxLineVel_ratio;
    double timeAxisAcceleration_ratio;
    E_WAVE_STATUS basicStatus;
    WeaveMotionCondition weaveCondition;
    AxisObject* speedRatioAxis;//速度比例轴
    MotionMessage *motionMessage;
    double haltTimeCount;
    int stopCount_left;
    int stopCount_right;
    double theaAxisTime;
    int haltStage;

};

#endif // WAVEMOTION_H
